#include "datadealer.h"
#include <QThread>
#include <QObject>
#include <rosbag/bag.h>
#include <rosbag/view.h>
#include <tf2_msgs/TFMessage.h>
#include<boost/foreach.hpp>


void PoseWorker::doWork(const QString &bagfilename, const QString &targetDir,const QString &frame_id, const QString &child_frame_id)
{
    QJsonArray ego_pose;
    rosbag::Bag bag;
    if(bag.isOpen() == false)
        bag.open(bagfilename.toStdString(), rosbag::bagmode::Read);
    rosbag::View view(bag, rosbag::TopicQuery("/tf"));
    BOOST_FOREACH(rosbag::MessageInstance const m, view)
    {
        tf2_msgs::TFMessageConstPtr tfStamped = m.instantiate<tf2_msgs::TFMessage>();
        for(int cnt = 0; cnt < tfStamped->transforms.size(); ++cnt)
        {

            QString father_frame = QString::fromStdString(tfStamped->transforms.at(cnt).header.frame_id);
            QString child_frame = QString::fromStdString(tfStamped->transforms.at(cnt).child_frame_id);
            if(father_frame == frame_id && child_frame == child_frame_id)
            {
                auto Nsec = tfStamped->transforms.at(cnt).header.stamp.toNSec();
                auto Rx = tfStamped->transforms.at(cnt).transform.rotation.x;
                auto Ry = tfStamped->transforms.at(cnt).transform.rotation.y;
                auto Rz = tfStamped->transforms.at(cnt).transform.rotation.z;
                auto Rw = tfStamped->transforms.at(cnt).transform.rotation.w;
                auto Tx = tfStamped->transforms.at(cnt).transform.translation.x;
                auto Ty = tfStamped->transforms.at(cnt).transform.translation.y;
                auto Tz = tfStamped->transforms.at(cnt).transform.translation.z;
                QJsonObject tmp_dict;
                QString token = QString(QCryptographicHash::hash(QString::number(Nsec).toUtf8(),
                                                                 QCryptographicHash::Md5).toHex());
                QJsonArray rot = QJsonArray({QJsonValue(Rw), QJsonValue(Rx), QJsonValue(Ry), QJsonValue(Rz)});
                QJsonArray trans = QJsonArray({QJsonValue(Tx), QJsonValue(Ty), QJsonValue(Tz)});
                tmp_dict.insert("token", token);
                tmp_dict.insert("timestamp", static_cast<qint64>(Nsec/1000));
                tmp_dict.insert("rotation", rot);
                tmp_dict.insert("translation", trans);
                ego_pose.append(tmp_dict);
            }
        }
        //        break;
    }

    QJsonDocument doc;
    QString database(targetDir + "/database");
    QDir databaseDir(database);
    if(!databaseDir.exists())
        databaseDir.mkdir(database);
//    else
//    {
//        QDir.removeRecursively();
//        QDir.mkdir(database);
//    }
    QFile file(database + "/ego_pose.json");
    if(!file.open(QIODevice::WriteOnly | QIODevice::Text))
        qDebug() << "Failed to open file";
    QTextStream ts(&file);
    doc.setArray(ego_pose);
    QByteArray bytearray = doc.toJson();
    QString text = QString(bytearray);
    ts.setCodec("UTF-8");
    ts << text;
    file.close();

}


void ImageWorker::doWork(const QString &bagfilename, const QString &targetDir, const QString &topic)
{
    QJsonArray sensor_img;
    rosbag::Bag bag;
    bag.open(bagfilename.toStdString(), rosbag::bagmode::Read);
    QString topicUpper = QString(topic).remove(0, 1).replace("/", "_").toUpper();
    QString topicImgPath = targetDir + "/" + topicUpper;
    QDir imgDir(topicImgPath);
    if(imgDir.exists())
        imgDir.removeRecursively();
    imgDir.mkdir(topicImgPath);
    rosbag::View view(bag, rosbag::TopicQuery(topic.toStdString()));
    BOOST_FOREACH(rosbag::MessageInstance const m, view)
    {
        sensor_msgs::ImagePtr rosImg = m.instantiate<sensor_msgs::Image>();
        auto Nsec =  rosImg->header.stamp.toNSec();
        cv_bridge::CvImagePtr cv_ptr = cv_bridge::toCvCopy(rosImg, rosImg->encoding);
        cv::Mat cvImg = cv_ptr->image;
        QString imgFileName = QString::number(Nsec/1000) + ".jpg";
        QString imgPath = topicImgPath + "/" + imgFileName;
        QJsonObject tmp_dict;
        QString token = QString(QCryptographicHash::hash(QString::number(Nsec).toUtf8(),
                                                         QCryptographicHash::Md5).toHex());
        tmp_dict.insert("token", token);
        tmp_dict.insert("timestamp", static_cast<qint64>(Nsec/1000));
        tmp_dict.insert("filename", topicUpper + "/" + imgFileName);
        sensor_img.append(tmp_dict);
        cv::imwrite(imgPath.toStdString(), cvImg);
    }
    QJsonDocument doc;
    QString database(targetDir + "/database");
    QDir databaseDir(database);
    if(!databaseDir.exists())
        databaseDir.mkdir(database);

    QFile file(database + "/" + topicUpper + ".json");
    if(!file.open(QIODevice::WriteOnly | QIODevice::Text))
        qDebug() << "Failed to open file";
    QTextStream ts(&file);
    doc.setArray(sensor_img);
    QByteArray bytearray = doc.toJson();
    QString text = QString(bytearray);
    ts.setCodec("UTF-8");
    ts << text;
    file.close();
}

void PointCloudWorker::doWork(const QString &bagfilename, const QString &targetDir, const QString &topic)
{
    QJsonArray sensor_pc;
    rosbag::Bag bag;
    bag.open(bagfilename.toStdString(), rosbag::bagmode::Read);
    QString topicUpper = QString(topic).remove(0, 1).replace("/", "_").toUpper();
    QString topicPCPath = targetDir + "/" + topicUpper;
    QDir pCDir(topicPCPath);
    if(pCDir.exists())
        pCDir.removeRecursively();
    pCDir.mkdir(topicPCPath);
    rosbag::View view(bag, rosbag::TopicQuery(topic.toStdString()));
    pcl::PCDWriter writer;
    BOOST_FOREACH(rosbag::MessageInstance const m, view)
    {
        sensor_msgs::PointCloud2Ptr msg = m.instantiate<sensor_msgs::PointCloud2>();
        pcl::PointCloud<pcl::PointXYZ> cloud;
        pcl::fromROSMsg(*msg, cloud);
        auto Nsec = msg->header.stamp.toNSec();
        QString pCFileName = QString::number(Nsec/1000) + ".pcd.bin";
        QString pCPath = topicPCPath + "/" + pCFileName;
        QJsonObject tmp_dict;
        QString token = QString(QCryptographicHash::hash(QString::number(Nsec).toUtf8(),
                                                                 QCryptographicHash::Md5).toHex());
        tmp_dict.insert("token", token);
        tmp_dict.insert("timestamp", static_cast<qint64>(Nsec/1000));
        tmp_dict.insert("filename", topicUpper + "/" + pCFileName);
        sensor_pc.append(tmp_dict);
        writer.writeBinaryCompressed(pCPath.toStdString(), cloud);
    }
    QJsonDocument doc;
    QString database(targetDir + "/database");
    QDir databaseDir(database);
    if(!databaseDir.exists())
        databaseDir.mkdir(database);

    QFile file(database + "/" + topicUpper + ".json");
    if(!file.open(QIODevice::WriteOnly | QIODevice::Text))
        qDebug() << "Failed to open file";
    QTextStream ts(&file);
    doc.setArray(sensor_pc);
    QByteArray bytearray = doc.toJson();
    QString text = QString(bytearray);
    ts.setCodec("UTF-8");
    ts << text;
    file.close();
}


/*
This is a prompt for the class
***************************
***************************
***************************
***************************
*/
PoseDataDealer::PoseDataDealer()
{
    PoseWorker *worker = new PoseWorker;
    worker->moveToThread(&workerThread);
    connect(&workerThread, &QThread::finished, worker, &QObject::deleteLater);
    connect(this, &PoseDataDealer::operate, worker, &PoseWorker::doWork);
    //    connect(worker, &PoseWorker::resultReady, this, &PoseDataDealer::workHandle);
    workerThread.start();
}

ImageDataDealer::ImageDataDealer()
{
    ImageWorker *worker = new ImageWorker;
    worker->moveToThread(&workerThread);
    connect(&workerThread, &QThread::finished, worker, &QObject::deleteLater);
    connect(this, &ImageDataDealer::operate, worker, &ImageWorker::doWork);
    //    connect(worker, &PoseWorker::resultReady, this, &PoseDataDealer::workHandle);
    workerThread.start();
}


PointCloudDataDealer::PointCloudDataDealer()
{
    PointCloudWorker *worker = new PointCloudWorker;
    worker->moveToThread(&workerThread);
    connect(&workerThread, &QThread::finished, worker, &QObject::deleteLater);
    connect(this, &PointCloudDataDealer::operate, worker, &PointCloudWorker::doWork);
    //    connect(worker, &PoseWorker::resultReady, this, &PoseDataDealer::workHandle);
    workerThread.start();
}

/*
This is a prompt for the class
***************************
***************************
***************************
***************************
*/
PoseDataDealer::~PoseDataDealer()
{
    workerThread.quit();
    workerThread.wait();
}


ImageDataDealer::~ImageDataDealer()
{
    workerThread.quit();
    workerThread.wait();
}

PointCloudDataDealer::~PointCloudDataDealer()
{
    workerThread.quit();
    workerThread.wait();
}



/*
This is a prompt for the class
***************************
***************************
***************************
***************************
*/
void PoseDataDealer::process(const QString &bagfilename, const QString &targetDir, const QString &fromTf, const QString &toTf)
{
    emit operate(bagfilename, targetDir, fromTf, toTf);
}

void ImageDataDealer::process(const QString &bagfilename, const QString &targetDir, const QString &topic)
{
    emit operate(bagfilename, targetDir, topic);
}

void PointCloudDataDealer::process(const QString &bagfilename, const QString &targetDir, const QString &topic)
{
    emit operate(bagfilename, targetDir, topic);
}
